#ifndef __STEP_MOTOR_DRV_H
#define __STEP_MOTOR_DRV_H

#include "./simple_step_motor_drv.h"

namespace driver {

template <typename CFG> class StepMotorDrv : protected SimpleStepMotorDrv<CFG> {

public:
    using BASE_TYPE = SimpleStepMotorDrv<CFG>;

    void init() {
        BASE_TYPE::init();
        BASE_TYPE::m_flags.set_flags(BASE_TYPE::FLAG_RUN_FINISH);
    }

    void on_puls() { BASE_TYPE::on_puls(); }

    uint8_t reset() {
        BASE_TYPE::m_flags.wait_any_flags(BASE_TYPE::FLAG_RUN_FINISH);
        uint8_t ec = BASE_TYPE::reset();
        if (ec == 0) {
            m_position = 0;
        }
        BASE_TYPE::m_flags.set_flags(BASE_TYPE::FLAG_RUN_FINISH);
        return ec;
    }

    uint8_t wait_for() {
        uint8_t ec = 0;
        BASE_TYPE::m_flags.wait_any_flags(BASE_TYPE::FLAG_RUN_FINISH);

        if (CFG::is_alm()) {
            ec = CFG::EC_ALM;
        } else if (m_position == 0) {
            osDelay(50);
            ec = BASE_TYPE::reset_slow();
        }
        m_position_valid = (ec == 0);
        BASE_TYPE::m_flags.set_flags(BASE_TYPE::FLAG_RUN_FINISH);
        return ec;
    }

    uint8_t run_target(int32_t position) {
        uint8_t ec = async_run_target(position);
        if (ec != 0) {
            return ec;
        }
        return wait_for();
    }

    uint8_t async_run_target(int32_t position) {
        uint8_t ec = 0;

        BASE_TYPE::m_flags.wait_any_flags(BASE_TYPE::FLAG_RUN_FINISH);
        if (!m_position_valid) {
            ec = BASE_TYPE::reset();
            if (ec != 0) {
                BASE_TYPE::m_flags.set_flags(BASE_TYPE::FLAG_RUN_FINISH);
                return ec;
            }
            m_position_valid = true;
            m_position = 0;
        }

        if (position > m_position) {
            CFG::set_dir(SimpleDir::EXIT);
            BASE_TYPE::run_total(position - m_position);
        } else if (position < m_position) {
            CFG::set_dir(SimpleDir::ENTER);
            BASE_TYPE::run_total(m_position - position);
        } else {
            BASE_TYPE::m_flags.set_flags(BASE_TYPE::FLAG_RUN_FINISH);
        }

        m_position = position;
        return ec;
    }

private:
    int32_t m_position = 0;
    bool m_position_valid = false;
};

} // namespace driver

#endif
